
#ifndef _MEMBER_BSOFTBODY_H_
#define _MEMBER_BSOFTBODY_H_

#include <BSoftBody>
#include <BRestraint>
#include <BVectorArray>
#include "IOverlapPair.h"
#include "ITriangleShape.h"
#include "ISparseSDF.h"
#include "ISpaceTree.h"

#define softbody_member(softbody)		((member_BSoftBody*)((BSoftBody*)softbody)->_ptr)

namespace BWE
{
	struct ISoftInfo
	{
		BReal			air_density = (BReal)1.2;
		BReal			water_density = 0;
		BReal			water_offset = 0;
		BReal			maxDisplacement = BReal(1000);
		BVector			water_normal;
		ISparseSdf		sparsesdf;
		IBroadphase*	broadphase = 0;
	};

	enum eAeroModel
	{
		V_Point,             //Vertex normals are oriented toward velocity
		V_TwoSided,          //Vertex normals are flipped to match velocity
		V_TwoSidedLiftDrag,  //Vertex normals are flipped to match velocity and lift and drag forces are applied
		V_OneSided,          //Vertex normals are taken as it is
		F_TwoSided,          //Face normals are flipped to match velocity
		F_TwoSidedLiftDrag,  //Face normals are flipped to match velocity and lift and drag forces are applied
		F_OneSided,          //Face normals are taken as it is
	};

	struct ContactInfo
	{
		BBody* body = 0;		// Rigid body
		BVector	normal;			// Outward normal
		BReal		offset = 0;		// Offset from origin
		BVector	bary;			// Barycentric weights for faces
	};

	struct Medium
	{
		BVector	velocity;
		BReal		pressure;
		BReal		density;
	};

	struct Vertex
	{
		BVector		position;	// Position
		BVector		prepos;		// Previous step position/Test position
		BVector		velocity;	// Velocity
		BVector		prevel;		// Previous step velocity
		BVector		force;		// Force accumulator
		BVector		normal;		// Normal
		BReal		mass = 0;
		BReal		imass = 0;
		BReal		area = 0;
		SpaceNode*	leaf = 0;
		int			penetration = 0;   // depth of penetration
		int			index = 0;
		BVector		m_splitv;				// velocity associated with split impulse
		BMatrix	m_effectiveMass;		// effective mass in contact
		BMatrix	m_effectiveMass_inv;	// inverse of effective mass
	};

	struct Link
	{
		Vertex* vertex0 = 0;	// Vertex pointers
		Vertex* vertex1 = 0;	// Vertex pointers
		BReal		length;			// Rest length
		BReal		length2;		// rl^2
	};

	struct Face
	{
		Vertex* vertex(int index)
		{
			if (index == 0) return vertex0;
			if (index == 1) return vertex0;
			if (index == 2) return vertex0;
			return 0;
		}
		Vertex* vertex0 = 0;
		Vertex* vertex1 = 0;
		Vertex* vertex2 = 0;
		BVector			center;
		BVector			normal;			// Normal
		BReal			area = 0;		// Rest area
		SpaceNode* leaf = 0;		// Leaf data
		BPlane			baryPlane;		// barycentric weights of the persistent contact
	};

	struct VertexContact
	{
		ContactInfo		info;			// Contact infos
		Vertex*			vertex = 0;		// Owner vertex
		BMatrix		impulseMatrix;	// Impulse matrix
		BVector			localPoint;		// Relative anchor
		BReal			friction;		// Friction
		BReal			hardness;		// Hardness

		BVector t1;
		BVector t2;
	};

	class RigidContact
	{
	public:
		ContactInfo		info;			// Contact infos
		BMatrix			impulseMatrix;	// Impulse matrix
		BVector			localPoint;		// Relative anchor
		BReal			imass;		// inverse mass of node/face
		BReal			friction;	// Friction
		BReal			hardness;	// Hardness
		BMatrix			imassMatrix;// inverse effective mass

		BVector t1;
		BVector t2;
	};

	class VertexRigidContact : public RigidContact
	{
	public:
		Vertex* vertex = 0;  // Owner node
	};

	class VertexRigidAnchor : public VertexRigidContact
	{
	public:
		BVector m_local;  // Anchor position in body space
	};

	class FaceRigidContact : public RigidContact
	{
	public:
		Face*	face = 0;	// Owner face
		BVector	point;		// Contact point
		BVector	bary;		// Barycentric weights
		BVector	weight;
	};

	struct FaceVertexContact
	{
		BBody*		body = 0;		// Collision object to collide with.
		Vertex*		vertex = 0;		// Vertex
		Face*		face = 0;		// Face
		BVector		bary;			// Barycentric weights
		BVector		weights;		// v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v;
		BVector		normal;			// Normal
		BReal		margin = 0;		// Margin
		BReal		friction = 0;	// Friction
		BReal		faceIMass = 0;		// inverse mass of the face at contact point
		BReal		scaleImpulse = 0;	// scale of the impulse matrix;
	};

	struct SoftContact : public BThing
	{
		Vertex*		node = 0;	// Vertex
		Face*		face = 0;	// Face
		BVector		weights;	// Weigths
		BVector		normal;		// Normal
		BReal		margin;		// Margin
		BReal		friction;	// Friction
		BReal		forceMixing[2];    // Restraint force mixing
	};

	struct Anchor
	{
		Vertex*		vertex = 0;
		BBody*		body = 0;
		BVector		pivot;
		BReal		influence = 0;
		inline bool operator == (const BBody* body) const
		{
			return this->body == body;
		}
	};

	struct Pose
	{
		bool			isframe;			// Is frame
		BReal			space;			// Rest space
		BReal			matching;		// Pose matching coefficient [0,1]
		BVectorArray	positions;		// Reference positions
		BRealArray		weights;		// Weights
		BVector			center;			// COM
		BMatrix			rotation;		// Rotation
		BMatrix			scale;			// scaling
		BMatrix			base_scaling;	// Base scaling
	};

	struct Cluster
	{
		BRealArray		masses;
		BArray<Vertex*>	vertexs;
		BVectorArray	framerefs;
		BMatrix			framexform;
		BReal			idmass = 0;
		BReal			imass = 0;
		BMatrix			locii;
		BVector			invInertia;
		BVector			com;
		BVector			vimpulses[2];
		BVector			dimpulses[2];
		int				nvimpulses = 0;
		int				ndimpulses = 0;
		BVector			lv;
		BVector			av;
		SpaceNode*		leaf = 0;
		BReal			node_damping = 0;		// Vertex damping
		BReal			linear_damping = 0;		// Linear damping
		BReal			angular_damping = 0;	// Angular damping
		BReal			matching = 0;
		BReal			maxSelfCollisionImpulse = 100;
		BReal			selfCollisionImpulseFactor = BReal(0.01);
		bool			containsAnchor = false;
		bool			collide = 0;
		int				clusterIndex = 0;
	};

	struct Impulse
	{
		BVector		velocity;
		BVector		drift;
		int			asVelocity = 0;
		int			asDrift = 0;
		Impulse operator - () const
		{
			Impulse i = *this;
			i.velocity = -i.velocity;
			i.drift = -i.drift;
			return (i);
		}
		Impulse operator * (BReal x) const
		{
			Impulse i = *this;
			i.velocity *= x;
			i.drift *= x;
			return (i);
		}
	};

	static void ClusterVImpulse(Cluster* cluster, const BVector& rpos, const BVector& impulse);
	static void ClusterDImpulse(Cluster* cluster, const BVector& rpos, const BVector& impulse);
	static void ClusterVAImpulse(Cluster* cluster, const BVector& impulse);
	static void ClusterDAImpulse(Cluster* cluster, const BVector& impulse);
	static void ClusterAImpulse(Cluster* cluster, const Impulse& impulse);
	static void ClusterDCImpulse(Cluster* cluster, const BVector& impulse);

	struct Body
	{
		BBody* body = 0;
		Cluster* cluster = 0;

		Body();
		Body(BBody* body);
		Body(Cluster* cluster);

		BReal invMass() const;
		BVector invInertia() const;
		const BMatrix& xform() const;
		BVector linearVelocity() const;
		BVector angularVelocity(const BVector& rpos) const;
		BVector angularVelocity() const;
		BVector velocity(const BVector& rpos) const;
		void applyVImpulse(const BVector& impulse, const BVector& rpos) const;
		void applyDImpulse(const BVector& impulse, const BVector& rpos) const;
		void applyImpulse(const Impulse& impulse, const BVector& rpos) const;
		void applyVAImpulse(const BVector& impulse) const;
		void applyDAImpulse(const BVector& impulse) const;
		void applyAImpulse(const Impulse& impulse) const;
		void applyDCImpulse(const BVector& impulse) const;
	};

	struct Joint : public BThing
	{
		enum eType
		{
			Linear = 0,
			Angular,
			Contact
		};
		struct Specs
		{
			BReal factor = 1;
			BReal forceMixing = 1;
			BReal split = 1;
		};

		virtual void Prepare(BReal dt, int iterations);
		virtual void Solve(BReal dt, BReal sor) = 0;
		virtual void terminate() = 0;
		virtual eType Type() const = 0;

		Body	bodies[2];
		BVector	refs[2];
		BReal	forceMixing;
		BReal	factor;
		BReal	split;
		BVector drift;
		BVector sdrift;
		BMatrix massmatrix;
	};

	struct LJoint : Joint
	{
		struct Specs : Joint::Specs
		{
			BVector position;
		};
		BVector rpos[2];
		void Prepare(BReal dt, int iterations);
		void Solve(BReal dt, BReal sor);
		void terminate();
		eType Type() const
		{
			return (eType::Linear);
		}
	};

	struct AJoint : Joint
	{
		struct IControl
		{
			virtual ~IControl() {}
			virtual void Prepare(AJoint*) {}
			virtual BReal Speed(AJoint*, BReal current) { return (current); }
			static IControl* Default()
			{
				static IControl def;
				return (&def);
			}
		};
		struct Specs : Joint::Specs
		{
			Specs() : icontrol(IControl::Default()) {}
			BVector axis;
			IControl* icontrol;
		};
		BVector		axis[2];
		IControl*	icontrol;
		void Prepare(BReal dt, int iterations);
		void Solve(BReal dt, BReal sor);
		void terminate();
		eType Type() const
		{
			return (eType::Angular);
		}
	};

	struct CJoint : Joint
	{
		int		life;
		int		maxlife;
		BVector rpos[2];
		BVector normal;
		BReal	friction;
		void Prepare(BReal dt, int iterations);
		void Solve(BReal dt, BReal sor);
		void terminate();
		eType Type() const
		{
			return (eType::Contact);
		}
	};

	typedef BHolder<Joint>	JointHolder;

	class member_BSoftBody
	{
	public:
		member_BSoftBody(BSoftBody* softBody);
		~member_BSoftBody();

		void appendLink(int node0, int node1, bool bcheckexist = false);
		void appendFace(int node0, int node1, int node2);

		void updateSpaceTree();
		void updateConstants();
		void updateFace(SpaceNode* node, bool use_velocity, bool use_margin);

		void evaluateMedium(const BVector& x, Medium& medium);
		bool proximityTest(const BVector& x1, const BVector& x2, const BVector& x3, const BVector& x4, const BVector& normal, const BReal& mrg, BVector& bary);
		BVector generateUnitOrthogonalVector(const BVector& u);

		bool checkContact(BBody* body, BShape* shape, const BMatrix& matrix, const BVector& point, BReal margin, ContactInfo& cti) const;
		bool checkContact(BBody* body, BShape* shape, const BMatrix& matrix, Face& face, BVector& point, BVector& bary, BReal margin, ContactInfo& cti) const;

		bool checkLink(const Vertex* node0, const Vertex* node1) const;

		void collisionBody(Vertex* vertex, BBody* body, BShape* shape, const BMatrix& matrix, BReal margin);
		void collisionBody(Face* face, BBody* body, BShape* shape, const BMatrix& matrix, BReal margin);

		void appendLinearJoint(const LJoint::Specs& specs, Cluster* bodyA, Body bodyB);
		void appendLinearJoint(const LJoint::Specs& specs, Body body = Body());
		void appendLinearJoint(const LJoint::Specs& specs, BSoftBody* body);

		void appendAngularJoint(const AJoint::Specs& specs, Cluster* bodyA, Body bodyB);
		void appendAngularJoint(const AJoint::Specs& specs, Body body = Body());
		void appendAngularJoint(const AJoint::Specs& specs, BSoftBody* body);

		void collisionSoftBody(const SpacePair& pair, BSoftBody* softBody1, BSoftBody* softBody2, BReal mrg, bool useFaceNormal);
		void collisionSoftBody(const SpacePair& pair, BSoftBody* softBody0, BSoftBody* softBody1, BReal mrg);
		void collisionSoftBody(const DbvntPair& pair, BSoftBody* softBody0, BSoftBody* softBody1, BReal margin, bool useFaceNormal);
		void process(const SpaceNode* la, const SpaceNode* lb, BSoftBody* softBody0, BSoftBody* softBody1, BReal margin, BReal friction);

		int clusterCount() const;
		static BVector clusterCom(const Cluster* cluster);

		void releaseCluster(int index);
		void releaseClusters();

		bool SolveContact(const IGjkEpaSolver::Result& result, Body ba, const Body bb, CJoint& joint, BReal margin, BReal friction);
		void Repel(Face* f1, Face* f2, BSoftBody* softBody0, BSoftBody* softBody1, BReal margin, bool useFaceNormal);

	public:
		BSoftBody*		boss;
		int				clusterSolverCount;
		BReal			dtime = 0;
		BReal			idtime = 0;		// 1/sdtime
		BReal			velmrg = 0;		// velocity margin
		BReal			radmrg = 0;		// radial margin
		BReal			updmrg = 0;		// Update margin

		eAeroModel		aero_model;			// Aerodynamic model (default: V_Point)
		BReal			vel_factor;			// Velocities correction factor (Baumgarte)
		BReal			drag_coefficient;		// Drag coefficient [0,+inf]
		BReal			lift_coefficient;		// Lift coefficient [0,+inf]
		BReal			pressure_coefficient;	// Pressure coefficient [-inf,+inf]
		BReal			volume_coefficient;		// Volume conversation coefficient [0,+inf]
		BReal			soft_hardness;		// Soft contacts hardness [0,1]
		BReal			anchor_hardness;	// Anchors hardness [0,1]
		BReal			kSRHR_CL;			// Soft vs rigid hardness [0,1] (cluster only)
		BReal			kSKHR_CL;			// Soft vs kinetic hardness [0,1] (cluster only)
		BReal			kSSHR_CL;			// Soft vs soft hardness [0,1] (cluster only)
		BReal			kSR_SPLT_CL;		// Soft vs rigid impulse split [0,1] (cluster only)
		BReal			kSK_SPLT_CL;		// Soft vs rigid impulse split [0,1] (cluster only)
		BReal			kSS_SPLT_CL;		// Soft vs rigid impulse split [0,1] (cluster only)
		BReal			maxvolume;			// Maximum space ratio for pose

		SpaceNodeArray		result;
		SpacePairArray		spacePairs;
		DbvntPairArray		dbvntPairs;
		ISoftInfo*			worldInfo;

		bool					dirty;
		Pose					pose;
		BArray<Vertex>			vertexs;
		BArray<Link>			links;
		BArray<Face>			faces;
		BArray<Anchor>			anchors;
		BArray<VertexContact>	vertexContacts;
		BArray<VertexContact>	multi_contacts;
		BArray<VertexRigidAnchor>	vertexRigidAnchors;
		BArray<FaceVertexContact>	faceVertexContacts;
		BArray<FaceRigidContact>	faceRigidContacts;
		BArray<SoftContact>			soft_contacts;		// Soft contacts
		BArray<JointHolder>			joints;				// Joints
		BReal						timeacc;			// Time accumulator
		BSpace					space;				// Spatial bounds
		ISpaceTree				vertex_tree;		// Nodes tree
		ISpaceTree				face_tree;			// Faces tree
		DbvntNode*				face_dbvnt;			// Faces tree with normals
		ISpaceTree				cluster_dbvt;		// Clusters tree
		BArray<Cluster*>		clusters;			// Clusters
		BArray<BVector>			deltas;
		BArray<BReal>			weights;
		BReal					sleepingThreshold;
		BReal					maxSpeedSquared;
		BReal					repulsionStiffness;

		bool				selfCollision;
		BArray<bool>		clusterConnectivity;
		BVector				windVelocity;
		bool				reducedModel;
	};
}

#endif